#include "Code2_COMP.h"
#include "Code8_IMU.h"
#include "Code0_CONF_MOTION.h"

Bool ReceiceFromComp = FALSE;
//Bool SendToComp = TRUE;						 

unsigned char Comp_Receive[COMP_R_LENGTH] = {0};		 
unsigned char Comp_Send[COMP_S_LENGTH] = {0};

CompRXStruct CompRX;


extern SensorStruct Acc;
extern SensorStruct Mag;
extern SensorStruct Gyro;	
extern unsigned char DipMode;	
extern double azimuth[2];
extern unsigned char MOTOR11_L;
extern unsigned char MOTOR11_H;
extern unsigned char MOTOR12_L;
extern unsigned char MOTOR12_H;
extern unsigned char MOTOR13_L;
extern unsigned char MOTOR13_H;
extern unsigned char MOTOR14_L;
extern unsigned char MOTOR14_H;
//extern int MOTION_MODE_YuThesis;

void Comp_Init(void)
{
	Comp_Receive_Init();
	Comp_Send_Init();
	CompRX_Init();
} 

void Comp_Receive_Init(void)
{
	int i;

	for(i=0; i < COMP_R_LENGTH; i++)
	{
		Comp_Receive[i] = 0;
	}
}					 

void Comp_Send_Init(void)
{
	int i;

	for(i=0; i < COMP_S_LENGTH; i++)
	{
		Comp_Send[i] = 0;
	}
}
												 								 
void CompRX_Init(void)
{
	int i;

	for(i=0; i < COMP_RX_BUFF_SIZE; i++)
	{
		CompRX.Buff[i] = 0;
	}

	CompRX.EndIndex = 0;
	CompRX.NowIndex = 0;
}
	
Bool CompRX2Receive(void)
{
	int i;
	int scan_count = (int)(COMP_R_LENGTH / 2) + 1;
	unsigned char checksum = 0;

	while((CompRX.NowIndex != CompRX.EndIndex) && (scan_count > 0))
	{
		if(CompRX.Buff[((CompRX.NowIndex + COMP_R_HEAD1) % COMP_RX_BUFF_SIZE)] == 0xFF)
		{
			checksum = 0;
			for(i=(COMP_R_HEAD1+1); i < COMP_R_CHECKSUM; i++)
			{
				checksum += CompRX.Buff[((CompRX.NowIndex + i) % COMP_RX_BUFF_SIZE)];
			}	
			checksum = ~checksum;
	
			if(checksum == CompRX.Buff[((CompRX.NowIndex + COMP_R_CHECKSUM) % COMP_RX_BUFF_SIZE)])
			{
				for(i=0; i < COMP_R_LENGTH; i++)
				{
					Comp_Receive[i] = CompRX.Buff[((CompRX.NowIndex + i) % COMP_RX_BUFF_SIZE)];
				}

				Receive2Motion();
				CompRX.NowIndex = (CompRX.NowIndex + COMP_R_LENGTH) % COMP_RX_BUFF_SIZE;
				CompRX.NowIndex = 0;
				CompRX.EndIndex = 0;
				scan_count = 0;
				
				return TRUE;
			}
			else
			{
				CompRX.NowIndex = (CompRX.NowIndex + 1) % COMP_RX_BUFF_SIZE;
			}
		}
		else
		{
			CompRX.NowIndex = (CompRX.NowIndex + 1) % COMP_RX_BUFF_SIZE; 
		}
		scan_count--;
	}
	return FALSE;
}
			 

void CompSend(void)
{
	int i; 
	int temp0, temp1, temp2, temp3, temp4;
	
	temp0 = MotorData.Read[0][READ_LOW_POSITION] + (MotorData.Read[0][READ_HIGH_POSITION] << 8) - 
					MotorData_Correction[0];		 
	temp1 = MotorData.Read[1][READ_LOW_POSITION] + (MotorData.Read[1][READ_HIGH_POSITION] << 8) - 
					MotorData_Correction[1];
	temp2 = MotorData.Read[2][READ_LOW_POSITION] + (MotorData.Read[2][READ_HIGH_POSITION] << 8) - 
					MotorData_Correction[2];		 
	temp3 = MotorData.Read[3][READ_LOW_POSITION] + (MotorData.Read[3][READ_HIGH_POSITION] << 8) - 
					MotorData_Correction[3];
	temp4 = MotorData.Read[4][READ_LOW_POSITION] + (MotorData.Read[4][READ_HIGH_POSITION] << 8) - 
					MotorData_Correction[4];		 

	
		Comp_Send[COMP_S_HEAD1] = 0xFF;		 
		Comp_Send[COMP_S_HEAD2] = 0xFE;
		Comp_Send[COMP_S_DIP] = DipMode;
		Comp_Send[COMP_S_MOTOR_0_L] = temp0 & 0x00FF;
		Comp_Send[COMP_S_MOTOR_0_H] = (temp0 & 0xFF00) >> 8;
		Comp_Send[COMP_S_MOTOR_1_L] = temp1 & 0x00FF;
		Comp_Send[COMP_S_MOTOR_1_H] = (temp1 & 0xFF00) >> 8;
		Comp_Send[COMP_S_MOTOR_2_L] = temp2 & 0x00FF;
		Comp_Send[COMP_S_MOTOR_2_H] = (temp2 & 0xFF00) >> 8;
		Comp_Send[COMP_S_MOTOR_3_L] = temp3 & 0x00FF;
		Comp_Send[COMP_S_MOTOR_3_H] = (temp3 & 0xFF00) >> 8;
		Comp_Send[COMP_S_MOTOR_4_L] = temp4 & 0x00FF;
		Comp_Send[COMP_S_MOTOR_4_H] = (temp4 & 0xFF00) >> 8;
		Comp_Send[COMP_S_MOTION_DONE] = Motion.Doing;
		
//		for(i=COMP_S_ACC_X_L; i <= COMP_S_ACC_Z_H; i++)
//		{									
//			Comp_Send[i] = Acc.Buff[i-COMP_S_ACC_X_L];
//		}
//		for(i=COMP_S_GYRO_X_L; i <= COMP_S_GYRO_Z_H; i++)
//		{									
//			Comp_Send[i] = Gyro.Buff[i-COMP_S_GYRO_X_L];
//		}
//		for(i=COMP_S_MAG_X_L; i <= COMP_S_MAG_Z_H; i++)
//		{									
//			Comp_Send[i] = Mag.Buff[i-COMP_S_MAG_X_L];
//		}
		Comp_Send[COMP_S_AZIMUTH_L] = (unsigned short)azimuth[1] & 0x00FF;		 
		Comp_Send[COMP_S_AZIMUTH_H] = ((unsigned short)azimuth[1] & 0xFF00) >> 8;
	
		Comp_Send[COMP_S_CHECKSUM] = 0;
		for(i=COMP_S_HEAD2+1; i < COMP_S_CHECKSUM; i++)
		{
			Comp_Send[COMP_S_CHECKSUM] += Comp_Send[i];
		}
		Comp_Send[COMP_S_CHECKSUM] = ~Comp_Send[COMP_S_CHECKSUM];
	
		#ifdef COMP
			RS232_Send(COMP_PORT, Comp_Send, COMP_S_LENGTH);
		#endif

	}
//	RS232_Send(HMI_PORT, Comp_Send, COMP_S_LENGTH);	 

void Receive2Motion(void)
{
	if((Comp_Receive[COMP_R_MOTION_MODE] != NO_MOTION) && Motion.Replaceable) //////////////////////////////////////////
	{
		if((Comp_Receive[COMP_R_MOTION_MODE] > 110) && (Comp_Receive[COMP_R_MOTION_MODE] <= (100 + WALK_RAISEARM)))
		{
			Motion.Mode_RX = Comp_Receive[COMP_R_MOTION_MODE] - 100;
			Motion.Replaceable = FALSE;
		}
		else if((Comp_Receive[COMP_R_MOTION_MODE] > 10) && (Comp_Receive[COMP_R_MOTION_MODE] <= WALK_RAISEARM))
		{
			Motion.Mode_RX = Comp_Receive[COMP_R_MOTION_MODE];
		}
		else
		{
			Motion.Mode_RX = 0;
		}

		Motion.ContinueDistance_RX = (float)((int)(Comp_Receive[COMP_R_CONT_DIS_L] +
																				 (Comp_Receive[COMP_R_CONT_DIS_H] << 8)) - 32768) / 100;
		Motion.ContinueAngle_RX = (float)((int)(Comp_Receive[COMP_R_CONT_ANGLE_L] + 
																			(Comp_Receive[COMP_R_CONT_ANGLE_H] << 8)) - 32768) / 100;
//		
//		USART_SendData(DEBUG_PORT, Master_Receive[MASTER_R_CONT_ANGLE_L]);    
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}					
//		USART_SendData(DEBUG_PORT, Master_Receive[MASTER_R_CONT_ANGLE_H]);    
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}	
//		if(Motion.ContinueAngle_RX > 0)
//		{					
//			USART_SendData(DEBUG_PORT, 0);    
//			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//			{
//			}		
//		}
//		else
//		{			
//			USART_SendData(DEBUG_PORT, 1);    
//			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//			{
//		}		
//		}				
//		USART_SendData(DEBUG_PORT, abs((int)Motion.ContinueAngle_RX));    
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}												
	}

	if(Comp_Receive[COMP_R_HEAD_MODE] != NO_MOTION) //////////////////////////////////////////
	{	
		Head.Mode_RX = Comp_Receive[COMP_R_HEAD_MODE];						
		Head.Pos[0][0] = Comp_Receive[COMP_R_MOTOR_0_L];							
		Head.Pos[0][1] = Comp_Receive[COMP_R_MOTOR_0_H];
		Head.Pos[1][0] = Comp_Receive[COMP_R_MOTOR_1_L];
		Head.Pos[1][1] = Comp_Receive[COMP_R_MOTOR_1_H];		
	}	
	//*************************************For Yu's Thesis****************************************************
	
	if(Comp_Receive[COMP_R_MOTION_MODE] == THROW_MODE1 ) {			// MOTOR 11 & 12
	  Motion.ContinueDistance_RX = Motion.ContinueDistance_RX*12;
		Motion.ContinueAngle_RX = Motion.ContinueAngle_RX*12;
		MOTOR11_L = ((int)Motion.ContinueDistance_RX) & 0xff;							
		MOTOR11_H = ((int)Motion.ContinueDistance_RX) >> 8;
		MOTOR12_L = ((int)Motion.ContinueAngle_RX) & 0xff;							
		MOTOR12_H = ((int)Motion.ContinueAngle_RX) >> 8;
		//MOTION_MODE_YuThesis = 1;
	}	 	
	else if(Comp_Receive[COMP_R_MOTION_MODE] == THROW_MODE2 )	{	// MOTOR 11 & 13
		//MOTOR11_L = Comp_Receive[COMP_R_CONT_DIS_L];							
		//MOTOR11_H = Comp_Receive[COMP_R_CONT_DIS_H];
		//MOTOR13_L = Comp_Receive[COMP_R_CONT_ANGLE_L];							
		//MOTOR13_H = Comp_Receive[COMP_R_CONT_ANGLE_H];
		Motion.ContinueDistance_RX = Motion.ContinueDistance_RX*12;
		Motion.ContinueAngle_RX = Motion.ContinueAngle_RX*12;
		MOTOR11_L = ((int)Motion.ContinueAngle_RX) & 0xff;							
		MOTOR11_H = ((int)Motion.ContinueAngle_RX) >> 8;
		MOTOR13_L = ((int)Motion.ContinueDistance_RX) & 0xff;							
		MOTOR13_H = ((int)Motion.ContinueDistance_RX) >> 8; 
		//MOTION_MODE_YuThesis = 2;
	}	 	
	else if(Comp_Receive[COMP_R_MOTION_MODE] == THROW_MODE3 )	{	// MOTOR 11 & 14
		Motion.ContinueDistance_RX = Motion.ContinueDistance_RX*12;
		Motion.ContinueAngle_RX = Motion.ContinueAngle_RX*12;
		MOTOR11_L = ((int)Motion.ContinueAngle_RX) & 0xff;							
		MOTOR11_H = ((int)Motion.ContinueAngle_RX) >> 8;
		MOTOR14_L = ((int)Motion.ContinueDistance_RX) & 0xff;							
		MOTOR14_H = ((int)Motion.ContinueDistance_RX) >> 8;
		//MOTION_MODE_YuThesis = 3;
	}
	else if(Comp_Receive[COMP_R_MOTION_MODE] == THROW_MODE4 )	{	// MOTOR 12 & 13
		Motion.ContinueDistance_RX = Motion.ContinueDistance_RX*12;
		Motion.ContinueAngle_RX = Motion.ContinueAngle_RX*12;
		MOTOR12_L = ((int)Motion.ContinueDistance_RX) % 256;							
		MOTOR12_H = ((int)Motion.ContinueDistance_RX) / 256;
		MOTOR13_L = ((int)Motion.ContinueAngle_RX) % 256;							
		MOTOR13_H = ((int)Motion.ContinueAngle_RX) / 256;
		//MOTION_MODE_YuThesis = 4;
	}	
	else if(Comp_Receive[COMP_R_MOTION_MODE] == THROW_MODE5 )	{	// MOTOR 12 & 14
		Motion.ContinueDistance_RX = Motion.ContinueDistance_RX*12;
		Motion.ContinueAngle_RX = Motion.ContinueAngle_RX*12;
		MOTOR12_L = ((int)Motion.ContinueDistance_RX) % 256;							
		MOTOR12_H = ((int)Motion.ContinueDistance_RX) / 256;
		MOTOR14_L = ((int)Motion.ContinueAngle_RX) % 256;							
		MOTOR14_H = ((int)Motion.ContinueAngle_RX) / 256;
		//MOTION_MODE_YuThesis = 5;
	}
	else if(Comp_Receive[COMP_R_MOTION_MODE] == THROW_MODE6 )	{	// MOTOR 13 & 14
		Motion.ContinueDistance_RX = Motion.ContinueDistance_RX*12;
		Motion.ContinueAngle_RX = Motion.ContinueAngle_RX*12;
		MOTOR13_L = ((int)Motion.ContinueDistance_RX) % 256;							
		MOTOR13_H = ((int)Motion.ContinueDistance_RX) / 256;
		MOTOR14_L = ((int)Motion.ContinueAngle_RX) % 256;							
		MOTOR14_H = ((int)Motion.ContinueAngle_RX) / 256;
		//MOTION_MODE_YuThesis = 6;
	}	 				

//	USART_SendData(DEBUG_PORT, Comp_Receive[COMP_R_MOTION_MODE]);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}															
}
